import Joint from "./joint";
import CONSTANT from "../../constant";
import BasisTracker from "./basis-tracker";
import Vec3 from "../../common/vec3";
import Method from "../../common/method";

export default class PrismaticJoint extends Joint{
    constructor(config) {
        super(config,CONSTANT.JOINT_TYPE_PRISMATIC);
        let v = config.localAxis1.elements;
        this.localBasis1[0]=v[0];
        this.localBasis1[1]=v[1];
        this.localBasis1[2]=v[2];
        let v1 = config.localAxis2.elements;
        this.localBasis2[0]=v1[0];
        this.localBasis2[1]=v1[1];
        this.localBasis2[2]=v1[2];
        this.buildLocalBasesFromX();
        this._basis = new BasisTracker(this);
        this.linearError=new Float64Array(3);//0:translation,1:linearErrorY,2:linearErrorZ
        this.angularError=new Float64Array(3);
        this._sd = config.springDamper.clone();
        this._lm = config.limitMotor.clone();
        this._tm1=new Float64Array(9);
        this._tm2=new Float64Array(9);
    }

    //region
    get translation(){return this.linearError[0];}
    get linearErrorY(){return this.linearError[1];}
    get linearErrorZ(){return this.linearError[2];}
    set translation(n){this.linearError[0]=n;}
    set linearErrorY(n){this.linearError[1]=n;}
    set linearErrorZ(n){this.linearError[2]=n;}

    get angularErrorX(){return this.angularError[0];};
    get angularErrorY(){return this.angularError[1];};
    get angularErrorZ(){return this.angularError[2];};
    set angularErrorX(n){this.angularError[0]=n;};
    set angularErrorY(n){this.angularError[1]=n;};
    set angularErrorZ(n){this.angularError[2]=n;};
    //endregion

    getInfo(info,timeStep,isPositionPart) {
        let bs=this._basis.elements;
        let erp = this.getErp(timeStep,isPositionPart);
        let linE=this.linearError,angE=this.angularError;
        let linRhsY = linE[1] * erp, linRhsZ = linE[2] * erp;
        let angRhsX = angE[0] * erp, angRhsY = angE[1] * erp, angRhsZ = angE[2] * erp;
        let j;
        if(this._sd.frequency <= 0 || !isPositionPart) {
            let impulse = this._impulses[0];
            let row = info.rows[info.numRows++];
            this._resetRow(row,impulse);
            this.setSolverInfoRowLinear(row,linE[0],this._lm,1 / (this._b1._invMass + this._b2._invMass),this._sd,timeStep,isPositionPart);
            Method.setJacobian(bs[0],bs[1],bs[2],this.relativeAnchor1,this.relativeAnchor2,row.jacobian.elements);
        }
        let impulse = this._impulses[1];
        let row = info.rows[info.numRows++];
        this._resetRow(row,impulse);
        row.rhs = linRhsY;
        row.cfm = 0;
        row.minImpulse = -1e65536;
        row.maxImpulse = 1e65536;
        Method.setJacobian(bs[3],bs[4],bs[5],this.relativeAnchor1,this.relativeAnchor2,row.jacobian.elements);
        let impulse1 = this._impulses[2];
        let row1 = info.rows[info.numRows++];
        this._resetRow(row1,impulse1);
        row1.rhs = linRhsZ;
        row1.cfm = 0;
        row1.minImpulse = -1e65536;
        row1.maxImpulse = 1e65536;
        Method.setJacobian(bs[6],bs[7],bs[8],this.relativeAnchor1,this.relativeAnchor2,row1.jacobian.elements);
        let impulse2 = this._impulses[3];
        let row2 = info.rows[info.numRows++];
        this._resetRow(row2,impulse2);
        row2.rhs = angRhsX;
        row2.cfm = 0;
        row2.minImpulse = -1e65536;
        row2.maxImpulse = 1e65536;
        j = row2.jacobian.elements;
        j[6] = 1;j[7] = 0;j[8] = 0;
        j[9] = 1;j[10] = 0;j[11] = 0;
        let impulse3 = this._impulses[4];
        let row3 = info.rows[info.numRows++];
        this._resetRow(row3,impulse3);
        row3.rhs = angRhsY;
        row3.cfm = 0;
        row3.minImpulse = -1e65536;
        row3.maxImpulse = 1e65536;
        j = row3.jacobian.elements;
        j[6] = 0;j[7] = 1;j[8] = 0;
        j[9] = 0;j[10] = 1;j[11] = 0;
        let impulse4 = this._impulses[5];
        let row4 = info.rows[info.numRows++];
        this._resetRow(row4,impulse4);
        row4.rhs = angRhsZ;
        row4.cfm = 0;
        row4.minImpulse = -1e65536;
        row4.maxImpulse = 1e65536;
        j = row4.jacobian.elements;
        j[6] = 0;j[7] = 0;j[8] = 1;
        j[9] = 0;j[10] = 0;j[11] = 1;
    }
    _syncAnchors() {
        super._syncAnchors();
        let bt = this._basis;
        let bte=bt.elements;
        let btj=bt.joint;
        let bs1=btj.basis1;
        let bs2=btj.basis2;
        let invM1 = btj._b1._invMass;
        let invM2 = btj._b2._invMass;
        let tv=this._tv,tm=this._tm;
        let q2X, q2Y, q2Z, q2W;
        let d = bs1[0] * bs2[0] + bs1[1] * bs2[1] + bs1[2] * bs2[2];
        if(d < -0.999999999) {
            Method.vecToQuat(bs1[0],bs1[1],bs1[2],tv);
            q2X=tv[0];q2Y=tv[1];q2Z=tv[2];q2W=0;
        } else {
            let cX = bs1[1] * bs2[2] - bs1[2] * bs2[1];
            let cY = bs1[2] * bs2[0] - bs1[0] * bs2[2];
            let cZ = bs1[0] * bs2[1] - bs1[1] * bs2[0];
            let w = Math.sqrt((1 + d) * 0.5);
            d = 0.5 / w;
            cX *= d;cY *= d;cZ *= d;
            q2X = cX;q2Y = cY;q2Z = cZ;q2W = w;
        }
        let q1X = 0,q1Y = 0,q1Z = 0,q1W = 1;
        let d1 = q1X * q2X + q1Y * q2Y + q1Z * q2Z + q1W * q2W;
        if(d1 < 0) {
            d1 = -d1;
            q2X = -q2X;q2Y = -q2Y;q2Z = -q2Z;q2W = -q2W;
        }
        let slerpQX, slerpQY, slerpQZ, slerpQW;
        if(d1 > 0.999999) {
            let dqX = q2X - q1X,dqY = q2Y - q1Y,dqZ = q2Z - q1Z,dqW = q2W - q1W;
            q2X = q1X + dqX * (invM1 / (invM1 + invM2));
            q2Y = q1Y + dqY * (invM1 / (invM1 + invM2));
            q2Z = q1Z + dqZ * (invM1 / (invM1 + invM2));
            q2W = q1W + dqW * (invM1 / (invM1 + invM2));
            let l = q2X * q2X + q2Y * q2Y + q2Z * q2Z + q2W * q2W;
            if(l > 1e-32) l = 1 / Math.sqrt(l);
            slerpQX = q2X * l;slerpQY = q2Y * l;slerpQZ = q2Z * l;slerpQW = q2W * l;
        } else {
            let theta = invM1 / (invM1 + invM2) * Math.acos(d1);
            q2X += q1X * -d1;q2Y += q1Y * -d1;q2Z += q1Z * -d1;q2W += q1W * -d1;
            let l = q2X * q2X + q2Y * q2Y + q2Z * q2Z + q2W * q2W;
            if(l > 1e-32) l = 1 / Math.sqrt(l);
            q2X *= l;q2Y *= l;q2Z *= l;q2W *= l;
            let sin = Math.sin(theta), cos = Math.cos(theta);
            q1X *= cos;q1Y *= cos;q1Z *= cos;q1W *= cos;
            slerpQX = q1X + q2X * sin;slerpQY = q1Y + q2Y * sin;slerpQZ = q1Z + q2Z * sin;slerpQW = q1W + q2W * sin;
        }
        Method.quatToMat3(slerpQX,slerpQY,slerpQZ,slerpQW,tm);
        tv[0]=bs1[0];tv[1]=bs1[1];tv[2]=bs1[2];
        Method.rotateVec3(tv,tm);
        let newXX = tv[0], newXY = tv[1], newXZ = tv[2];
        let prevXX = bte[0],prevXY = bte[1],prevXZ = bte[2];
        let prevYX = bte[3],prevYY = bte[4],prevYZ = bte[5];
        let d2 = prevXX * newXX + prevXY * newXY + prevXZ * newXZ;
        if(d2 < -0.999999999) {
            Method.vecToQuat(prevXX,prevXY,prevXZ,tv);
            slerpQX=tv[0];slerpQY=tv[1];slerpQZ=tv[2];slerpQW=0;
        } else {
            let cX = prevXY * newXZ - prevXZ * newXY,cY = prevXZ * newXX - prevXX * newXZ,cZ = prevXX * newXY - prevXY * newXX;
            let w = Math.sqrt((1 + d2) * 0.5);
            d2 = 0.5 / w;
            cX *= d2;cY *= d2;cZ *= d2;
            slerpQX = cX;slerpQY = cY;slerpQZ = cZ;slerpQW = w;
        }
        Method.quatToMat3(slerpQX,slerpQY,slerpQZ,slerpQW,tm);
        tv[0]=prevYX;tv[1]=prevYY;tv[2]=prevYZ;
        Method.rotateVec3(tv,tm);
        let newYX = tv[0], newYY = tv[1], newYZ = tv[2];
        let newZX = newXY * newYZ - newXZ * newYY;
        let newZY = newXZ * newYX - newXX * newYZ;
        let newZZ = newXX * newYY - newXY * newYX;
        if(newZX * newZX + newZY * newZY + newZZ * newZZ > 1e-6) {
            let l = newZX * newZX + newZY * newZY + newZZ * newZZ;
            if(l > 0) l = 1 / Math.sqrt(l);
            newZX *= l;newZY *= l;newZZ *= l;
        } else {
            Method.vecToQuat(newXX,newXY,newXZ,tv);
            newZX=tv[0];newZY=tv[1];newZZ=tv[2];
        }
        newYX = newZY * newXZ - newZZ * newXY;newYY = newZZ * newXX - newZX * newXZ;newYZ = newZX * newXY - newZY * newXX;
        Method.setM3X3(bte,newXX,newXY,newXZ,newYX,newYY,newYZ,newZX,newZY,newZZ);
        let b1=Method.transposeM33(this.basis1,this._tm1);
        let b2=Method.transposeM33(this.basis2,this._tm2);
        Method.multiplyBasis(b1,b2,tm);
        Method.mat3ToQuat(tm,tv);
        let relQX=tv[0], relQY=tv[1], relQZ=tv[2], cosHalfTheta=tv[3];
        let theta = (cosHalfTheta <= -1 ? 3.14159265358979 : cosHalfTheta >= 1 ? 0 : Math.acos(cosHalfTheta)) * 2;
        let ae=this.angularError,le=this.linearError;
        ae[0] = relQX;ae[1] = relQY;ae[2] = relQZ;
        let l = ae[0] * ae[0] + ae[1] * ae[1] + ae[2] * ae[2];
        if(l > 0) l = 1 / Math.sqrt(l);
        ae[0] *= l;ae[1] *= l;ae[2] *= l;
        ae[0] *= theta;ae[1] *= theta;ae[2] *= theta;
        let anchorDiffX =this.anchor2[0] - this.anchor1[0];
        let anchorDiffY = this.anchor2[1] - this.anchor1[1];
        let anchorDiffZ = this.anchor2[2] - this.anchor1[2];
        le[0] = anchorDiffX * bte[0] + anchorDiffY * bte[1] + anchorDiffZ * bte[2];
        le[1] = anchorDiffX * bte[3] + anchorDiffY * bte[4] + anchorDiffZ * bte[5];
        le[2] = anchorDiffX * bte[6] + anchorDiffY * bte[7] + anchorDiffZ * bte[8];
    }
    _getVelocitySolverInfo(timeStep,info) {
        super._getVelocitySolverInfo(timeStep,info);
        this.getInfo(info,timeStep,false);
    }
    _getPositionSolverInfo(info) {
        super._getPositionSolverInfo(info);
        this.getInfo(info,null,true);
    }
    getAxis1() {
        return new Vec3(this.basis1[0],this.basis1[1],this.basis1[2]);
    }
    getAxis2() {
        return new Vec3(this.basis2[0],this.basis2[1],this.basis2[2]);
    }
    getAxis1To(axis) {
        let es=axis.elements;
        es[0] = this.basis1[0];es[1] = this.basis1[1];es[2] = this.basis1[2];
    }
    getAxis2To(axis) {
        let es=axis.elements;
        es[0] = this.basis2[0];es[1] = this.basis2[1];es[2] = this.basis2[2];
    }
    getLocalAxis1() {
        return new Vec3(this.localBasis1[0],this.localBasis1[1],this.localBasis1[2]);
    }
    getLocalAxis2() {
        return new Vec3(this.localBasis2[0],this.localBasis2[1],this.localBasis2[2]);
    }
    getLocalAxis1To(axis) {
        let es=axis.elements;
        es[0] = this.localBasis1[0];es[1] = this.localBasis1[1];es[2] = this.localBasis1[2];
    }
    getLocalAxis2To(axis) {
        let es=axis.elements;
        es[0] = this.localBasis2[0];es[1] = this.localBasis2[1];es[2] = this.localBasis2[2];
    }
    getSpringDamper() {
        return this._sd;
    }
    getLimitMotor() {
        return this._lm;
    }
    getTranslation() {
        return this.linearError[0];
    }
}